#include "Arduino.h"
#include "QB.h"
#include <Wire.h>


QB::QB()
{
  _lightN  = Light(LIGHT_NORTH);
  _lightE  = Light(LIGHT_EAST);
  _lightU  = Light(LIGHT_UP);
  _lightD  = Light(LIGHT_DOWN);
  _lightW  = Light(LIGHT_WEST);
  _lightS  = Light(LIGHT_SOUTH);
  _mSensor = MotionSensor(); //This is pretty static so we can probably do a some const int´s in a "global data" file to optimize for size
  _speaker = Speaker();
  _vibrator = Vibrator();
  _microphone = Microphone(&_mSensor);
  _xbee = Xbee(this);
  _pendingCommand = -1;
  _script = 0;
}

void QB::setColor(Color* c)
{
  char all[] = {'W', 'S', 'E', 'N', 'D', 'U'};
  setColor(c, all, 6); 
}

void QB::setColor(Color* c, char sides[], int num)
{
  for (int i=0; i<num; i++) {
    setColor(c, sides[i]);
  }
}

void QB::setColor(Color* c, char side)
{
  Light* light = getLight(side);
  light->setColor(c);
 }

void QB::setValue(int b)
{
  char all[] = {'W', 'S', 'E', 'N', 'D', 'U'};
  for (int i=0; i<6; i++) {
    setValue(b, all[i]);
  }
}

void QB::setValue(int b, char side)
{
  Light* light = getLight(side);
  light->setIntensity(b);
}

void QB::setHue(int b)
{
  char all[] = {'W', 'S', 'E', 'N', 'D', 'U'};
  for (int i=0; i<6; i++) {
    setHue(b, all[i]);
  }
}

void QB::setHue(int b, char side)
{
  Light* light = getLight(side);
  light->setHue(b);
}

void QB::setSaturation(int b)
{
  char all[] = {'W', 'S', 'E', 'N', 'D', 'U'};
  for (int i=0; i<6; i++) {
    setSaturation(b, all[i]);
  }
}

void QB::setSaturation(int b, char side)
{
  Light* light = getLight(side);
  light->setSaturation(b);
}

void QB::playAudioFile(int filenumber){
  _speaker.playAudioFile(filenumber);
}

void QB::stopPlaying(){
  _speaker.stopPlayBack();
}

void QB::setOn(boolean on)
{
  _on = on;
  
  _lightN.setOn(on);
  _lightS.setOn(on);
  _lightW.setOn(on);
  _lightE.setOn(on);
  _lightU.setOn(on);
  _lightD.setOn(on);
}

boolean QB::isPlayingAudio(){
	return _speaker.isBusy();
}

void QB::vibrate(int strength){
	_vibrator.vibrate(strength);
}

void QB::resetBlinkM(){
		delay(50);  // wait for things to stabilize 
		Wire.begin(); //Needed for i2c support
		//Stop all preloaded scripts
		Wire.beginTransmission(0);// join I2C, talk to ALL (0 means all) BlinkMs
		Wire.write('o');
		Wire.endTransmission(); 
		delay(50);
		//Turn off all lights
		Wire.beginTransmission(0);// 
		Wire.write('n');
        Wire.write((byte)0x00);             // value for red channel
  		Wire.write((byte)0x00);             // value for blue channel
		Wire.write((byte)0x00);             // value for green channel		
		Wire.endTransmission();   // leave I2C bus
		delay(30); //30 MS ur sumething to get the blinkms to behave
}

void QB::prepare() 
{
  _speaker.prepare();
  _mSensor.prepare();
  _vibrator.prepare();
  _microphone.prepare();
  resetBlinkM();
  //_xbee.prepare(); moved to own function
}

boolean QB::isOn() 
{
  return _on;
}

MotionSensor* QB::getMotionSensor()
{
  return &_mSensor;
}

Microphone* QB::getMicrophone()
{
  return &_microphone;
}

void QB::setQbId(char id){
	_xbee.setQbId(id);
}

char QB::getQbId(){
	return _xbee.getQbId();
}


void QB::setScript(int script){
  _script = script;
}

int QB::getScript(){
  return _script;
}

Light* QB::getLight(char side) 
{
  switch (side)
  {
    case 'W' : return &_lightW;
    case 'E' : return &_lightE;
    case 'S' : return &_lightS;
    case 'N' : return &_lightN;
    case 'U' : return &_lightU;
    case 'D' : return &_lightD;
  }
  return &_lightS;
}

String QB::getXbeeAdress(){
	return _xbee.getXbeeAdress();
}

void QB::prepareAndSetupXbee(){
	_xbee.prepare();
}


void QB::testForFitness(){
	//_mSensor.printRawXYZacceleration();
	//_vibrator.vibrate(255);

	_microphone.printRawData();
	
	
	//delay(100);
//	_speaker.playAudioFile(10);
};

void QB::broadcastState(int state, int value){
	_xbee.broadcastState(state, value);
	//also set internal state
}

void QB::broadcastCommand(char id, int data) {
  data = constrain(data, 0, 99);
	_xbee.broadcastCommand(id, data);
}

void QB::broadcastCommand(char id, char data_1, char data_2){
	_xbee.broadcastCommand(id, data_1, data_2);
}

void QB::updateSensors(){
	_mSensor.update();
	_microphone.update();
	_xbee.update(); //eg har lest at serial  er interrupt basert
};

void QB::stateChanged(char qbId, char stateId, int value) {

};

void QB::commandReceived(int command) {
  _pendingCommand = command;
};

void QB::enteredState(char stateId, int value) {
  
}

boolean QB::pendingCommandIs(int command) {
  if (_pendingCommand != command) { return false; }
  _pendingCommand = -1;
  return true;
}


    
